/**
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as
* published by the Free Software Foundation; either version 2 of
* the License, or (at your option) any later version.
*
* ArmarX is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package    ArmarX::
* @author     Mirko Waechter ( mirko.waechter at kit dot edu)
* @date       2014
* @copyright  http://www.gnu.org/licenses/gpl.txt
*             GNU General Public License
*/


#ifndef MMM_RIGIDBODYCONVERTER_H
#define MMM_RIGIDBODYCONVERTER_H

#include "../ConverterVicon2MMM/ConverterVicon2MMM.h"

#include <MMM/Motion/Legacy/Converter/MarkerBasedConverter.h>

#include <VirtualRobot/VirtualRobotCommon.h>
#include <VirtualRobot/Nodes/Sensor.h>

#include <Eigen/Dense>

namespace MMM {

    class RigidBodyConverter : public ConverterVicon2MMM
    {
    public:
        RigidBodyConverter(const std::string& name = "RigidBodyConverterVicon2MMM");


        // Converter interface
    public:
        AbstractMotionPtr initializeStepwiseConversion();
        bool convertMotionStep(AbstractMotionPtr currentOutput);

        /** @brief Finds the rigid body transform (\f$T\in SE_3\f$) that maps two point clouds with known correspondences onto each other.
         * @param A Matrix with the original points stored in the rows (3D or homogeneous 4D)
         * @param B Matrix with the transformed points stored in the rows (3D or homogeneous 4D)
         * @return A homogeneous transformation matrix
         * @details Obtains the homogeneous transformation matrix \f$T \in SE_3\f$ for which holds
         * \f[ T\cdot A^T = B^T. \f]
         * This is port from matlab as seen <a href="http://nghiaho.com/?page_id=671">here</a> as published by \cite besl_method_1992.
         */
        static Eigen::Matrix4f Registration3d(const Eigen::MatrixXf &A, const Eigen::MatrixXf &B );

        /** @brief Finds the rigid body transform (\f$T\in SE_3\f$) that maps two point clouds with known correspondences onto each other.
         * @param A Name/Position pairs of original points (Vector3f or Vector4f)
         * @param B Name/Position pairs of transformed points (Vector3f or Vector4f)
         * @return A homogeneous transformation matrix
         * @details A transformation matrix is obtained such that
         * \code{.cpp}
         * map<string,Vector4f> A,B;
         * string markerName;
         * Matrix4f T = registration3d(A,B);
         * T*A.at(markerName) == B.at(markerName);
         * \endcode
         * holds.
         */
        template <typename T>
        static Eigen::Matrix4f Registration3d(const std::map<std::string,T> &mA, const std::map<std::string,T> &mB );

        bool isInitialized();
    protected:

        bool buildModel(MMM::ModelPtr model);
    private:
        std::map<std::string, Eigen::Vector3f> modelMarkerPositions;
    };

    template <typename T>
    Eigen::Matrix4f RigidBodyConverter::Registration3d(const std::map<std::string,T> &mA, const std::map<std::string,T> &mB ){
        // Note: use T::Scalar if float or double is required.
        assert(mA.size()==mB.size());
        int n = T::RowsAtCompileTime;
        Eigen::MatrixXf A(mA.size(),n);
        Eigen::MatrixXf B(mA.size(),n);
        int row=0;
        for ( typename std::map<std::string,T>::const_iterator it=mA.begin(); it!=mA.end(); it++, row++){
            A.row(row)=it->second;
            B.row(row)=mB.at(it->first);
            //std::cout << it->first << " " << mB.at(it->first) << std::endl;
        }
        return Registration3d(A,B);
    }



}

#endif
